/**
 * Head of code common to all C targets
 **/
void config_run__(unsigned long tick);
void config_init__(void);
#if defined(windows_x86)
#include "iec_types.h"
#define LOG_LEVELS 4
#define LOG_CRITICAL 0
#define LOG_WARNING 1
#define LOG_INFO 2
#define LOG_DEBUG 3
#else
#include "beremiz.h"

#include"plc_app.h"

#endif
#include <string.h>

/*
 *  Variables used by generated C softPLC and plugins
 **/
IEC_TIME __CURRENT_TIME;
IEC_BOOL __DEBUG = 0;
extern unsigned long long common_ticktime__;
unsigned long __tick = 0;
char *PLC_ID = SWNAME;

/*
 *  Variable generated by C softPLC and plugins
 **/
extern unsigned long greatest_tick_count__;
/*
 * Prototypes of functions exported by plugins
 **/




int __init_motion();
void __cleanup_motion(void);
void __retrieve_motion(void);
void __publish_motion(void);
int __init_1();
void __cleanup_1(void);
void __retrieve_1(void);
void __publish_1(void);
int __init_2();
void __cleanup_2(void);
void __retrieve_2(void);
void __publish_2(void);
int __init_3();
void __cleanup_3(void);
void __retrieve_3(void);
void __publish_3(void);
int __init_4();
void __cleanup_4(void);
void __retrieve_4(void);
void __publish_4(void);



/*
 * Prototypes of functions provided by generated C softPLC
 **/



/*
 * Prototypes of functions provided by generated target C code
 * */

void __init_debug(void);
void __cleanup_debug(void);
/*void __retrieve_debug(void);*/
void __publish_debug(void);



/* Help to quit cleanly when init fail at a certain level */
static int init_level = 0;



/*
 * Retrieve input variables, run PLC and publish output variables
 **/
void __run(void)
{
    __tick++;
    if (greatest_tick_count__)
        __tick %= greatest_tick_count__;

    __retrieve_motion();
    __retrieve_1();
    __retrieve_2();
    __retrieve_3();
    __retrieve_4();

    /*__retrieve_debug();*/

    config_run__(__tick);
    __publish_debug();
    __publish_4();
    __publish_3();
    __publish_2();
    __publish_1();
    __publish_motion();

}

/*
 * Initialize variables according to PLC's default values,
 * and then init plugins with that values
 **/
int __init()
{
    int res = 0;
    init_level = 0;
    __tick=0;
    /* Effective tick time with 1ms default value */
    if(!common_ticktime__)
        common_ticktime__ = 1000000;

    config_init__();
    __init_debug();
    init_level=1; if((res = __init_motion())){return res;}
    init_level=2; if((res = __init_1())){return res;}
    init_level=3; if((res = __init_2())){return res;}
    init_level=4; if((res = __init_3())){return res;}
    init_level=5; if((res = __init_4())){return res;}
    return res;
}
/*
 * Calls plugin cleanup proc.
 **/
void __cleanup(void)
{
    if(init_level >= 5) __cleanup_4();
    if(init_level >= 4) __cleanup_3();
    if(init_level >= 3) __cleanup_2();
    if(init_level >= 2) __cleanup_1();
    if(init_level >= 1) __cleanup_motion();
    __cleanup_debug();
}
void PLC_GetTime(IEC_TIME *CURRENT_TIME);
void PLC_SetTimer(unsigned long long next, unsigned long long period);



/**
 * Tail of code common to all C targets
 **/

/** 
 * LOGGING
 **/

#ifndef TARGET_EXT_SYNC_DISABLE

#define CALIBRATED -2
#define NOT_CALIBRATED -1
static int calibration_count = NOT_CALIBRATED;
static IEC_TIME cal_begin;
static long long Tsync = 0;
static long long FreqCorr = 0;
static int Nticks = 0;
static unsigned long last_tick = 0;

/*
 * Called on each external periodic sync event
 * make PLC tick synchronous with external sync
 * ratio defines when PLC tick occurs between two external sync
 * @param sync_align_ratio 
 *          0->100 : align ratio
 *          < 0 : no align, calibrate period
 **/
void align_tick(int sync_align_ratio)
{
	/*
	printf("align_tick(%d)\n", calibrate);
	*/
	if(sync_align_ratio < 0){ /* Calibration */
		if(calibration_count == CALIBRATED)
			/* Re-calibration*/
			calibration_count = NOT_CALIBRATED;
		if(calibration_count == NOT_CALIBRATED)
			/* Calibration start, get time*/
			PLC_GetTime(&cal_begin);
		calibration_count++;
	}else{ /* do alignment (if possible) */
		if(calibration_count >= 0){
			/* End of calibration */
			/* Get final time */
			IEC_TIME cal_end;
			PLC_GetTime(&cal_end);
			/*adjust calibration_count*/
			calibration_count++;
			/* compute mean of Tsync, over calibration period */
			Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 +
					(cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count;
			if( (Nticks = (Tsync / common_ticktime__)) > 0){
				FreqCorr = (Tsync % common_ticktime__); /* to be divided by Nticks */
			}else{
				FreqCorr = Tsync - (common_ticktime__ % Tsync);
			}
			/*
			printf("Tsync = %ld\n", Tsync);
			printf("calibration_count = %d\n", calibration_count);
			printf("Nticks = %d\n", Nticks);
			*/
			calibration_count = CALIBRATED;
		}
		if(calibration_count == CALIBRATED){
			/* Get Elapsed time since last PLC tick (__CURRENT_TIME) */
			IEC_TIME now;
			long long elapsed;
			long long Tcorr;
			long long PhaseCorr;
			long long PeriodicTcorr;
			PLC_GetTime(&now);
			elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec;
			if(Nticks > 0){
				PhaseCorr = elapsed - (common_ticktime__ + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */
				Tcorr = common_ticktime__ + (PhaseCorr + FreqCorr) / Nticks;
				if(Nticks < 2){
					/* When Sync source period is near Tick time */
					/* PhaseCorr may not be applied to Periodic time given to timer */
					PeriodicTcorr = common_ticktime__ + FreqCorr / Nticks;
				}else{
					PeriodicTcorr = Tcorr;
				}
			}else if(__tick > last_tick){
				last_tick = __tick;
				PhaseCorr = elapsed - (Tsync*sync_align_ratio/100);
				PeriodicTcorr = Tcorr = common_ticktime__ + PhaseCorr + FreqCorr;
			}else{
				/*PLC did not run meanwhile. Nothing to do*/
				return;
			}
			/* DO ALIGNEMENT */
			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);
		}
	}
}

#endif
